#include "ai_fields.h"

void ai_make_normal()
{
 int max_rows = FIELD_HEIGTH/PIXEL_PROPORTION;
 int max_cols = FIELD_LENGTH/PIXEL_PROPORTION;

 int row = 0;
 int col = 0;
 int i;

 for (i=0;i<5;i++)
 {
 	//actual_state.ours[i]  <== position of one own robot. We have 5

 	for (row=0; row< max_rows; row ++)
		for (col=0; col < max_cols; col ++)
			{
				pos_field[row][col].norm_row = 90;
				//pos_field[row][col].norm_row = calcular la normal en funcio de la posicio
												 // dels contraris 
			}
 }
}


void ai_calcul_normal(int row, int col)
{
	pos_field[row][col].norm_row = 90;
}

// P1 = (row1, col1) own point
// Pg = (row2, col2) goal point

// N(p) = (Pg - P)/ abs(Pg - P)
// Normal vector is returned
void ai_calcul_position(int row1, int col1, int row2, int col2, int *rowN, int *colN)
{
	float angle;

		if (row1 != row2) *rowN = (double)(row2 - row1)/ (double)abs(row2 - row1); 
		else *rowN=0;

		if (col1 != col2) *colN = (double)(col2 - col1)/ (double)abs(col2 - col1); 
		else *colN=0;

		//printf("Pg: (%i,%i) ==> (%f,%f)\n",i,j,rowN,colN); 
	//colN = (double)(col2 - col1)/ (double)abs(col2 - col1);
//	printf("value N(p) (%i, %i) over (%i, %i) ==> (%f,%f)\n",row1,col1,row2,col2,rowN,colN);
	return;
}


void ai_calcul_Normal_position (int row1, int col1)
{
	int i=0;
	int j=0;
	int rowN;
	int colN;

	printf(" point P: (%i,%i): (row,col)\n",row1,col1); 
	for (i=0; i<6;i++){
		for(j=0;j<6;j++)
		{
		ai_calcul_position(row1, col1,	i, j,&rowN, &colN);
				  		
		printf("(%i,%i) ",(int)rowN,(int)colN);
		}
	printf("\n");
	}
}
